package dev;

import processing.core.PApplet;
import lll.wrj4P5.*;
import lll.Loc.*;

public class IrPen4P5 {

	  private PApplet parent;
	  private Loc[] pens; //refer to irLights[] of the given WiiRemote.
	  private Loc[] at = {new Loc(), new Loc(), new Loc(), new Loc()};
	  
	  boolean lastLighted=false, debug=false;
	  private int nOfClick=0;
	  private float S, T, sdX, sdY;
	  //  private float xOff=40./width, yOff=40./height;
	  //  private float xScl=(width-80.)/width, yScl=(height-80.)/height;
	  private int Sdir, Tdir;
	  private Loc Xax, Yax, Xv, Yv;

	  IrPen4P5(PApplet p5, WiiRimokon remote, boolean debug) {
	    pens = remote.irLights;
	    parent = p5;
	    this.debug = debug;
	  }

	  void sensorTest(){
	    parent.noFill();
	    parent.stroke(128);

	    parent.translate(parent.width/4, parent.height/4);
	    parent.rect(0,0,parent.width/2,parent.height/2);
	    parent.noStroke();
	    parent.fill(255,128,128);
	    for (int i=0;i<=1;i++) {
	      Loc p=pens[i];
	      if (p.x>-1) {
	    	  parent.ellipse(p.x*parent.width/2, (float) ((1.-p.y)*parent.height/2), p.z*50, p.z*100);
	      }
	    }
	    parent.translate(parent.width*4,parent.height*4);
	    //return true;
	  }

	  boolean isCalibrating() {



		  parent.noFill();
		  parent.stroke(128);

	    if (nOfClick>3) {
	      if (debug) drawIr(pens[0],parent.width,parent.height);
	      return false;
	    }

	    //draw crosshairs
	    int xx=(nOfClick%2)*(parent.width-80)+40, yy=(nOfClick/2)*(parent.height-80)+40;
	    parent.line(xx-7, yy, xx+7, yy);
	    parent.line(xx,yy-7,xx,yy+7);
	    parent.ellipse(xx, yy, 40, 40);


	    if (pens[0]==null || pens[0].x<0) {
	      lastLighted=false;
	      return true;
	    }

	    if (lastLighted) { 
	      return true; 
	    }

	    at[nOfClick].move(pens[0].x, pens[0].y, 0);

	    lastLighted=true;
	    nOfClick++;



	    if (nOfClick==4) { 
	      calculateFactors();
	    }
	    return true;



	  }

	  //end calibration
	  //begin 

	  //check how many pens are on
	  int npen() {
		    int n=0;
		    for (int i=0;i<4;n+=(pens[i].x>-1?1:0),i++);
		    return n;
		  }


	  //return the pens based on 0 to 3 for 1 to 4 pens connected
	  Loc pen(int id) {
	    if (id>=npen()) return null;
	    return toScreen(pens[id]);
	  }



	  private Loc toScreen(Loc pen) {
	    float x, y, sdP0;
	    float sdP1;
	    //Loc p = new Loc((pen.x+xOff)*xScl, (pen.y+yOff)*yScl, 0);

	    Loc p = new Loc(pen.x, pen.y, 0);

	    if (Xv!=null){
	      int Sp = (cross2D(p.sub(Xv), Xax)>0?1:-1);
	      y = Sdir*Sp*p.sub(Xv).angle(Xax)/S;
	    } 
	    else {
	      sdP0 = signedDist2D(p, at[0], at[1]);
	      sdP1 = signedDist2D(p, at[2], at[3]);
	      y = sdP0/sdY;
	    }


	    if (Yv!=null) {
	      int Tp = (cross2D(p.sub(Yv), Yax)>0?1:-1);
	      x = Tdir*Tp*p.sub(Yv).angle(Yax)/T;
	    } 
	    else {
	      sdP0 = signedDist2D(p, at[0], at[2]);
	      sdP1 = signedDist2D(p, at[1], at[3]);
	      x = sdP0/sdX;
	    }

	    if (debug) drawP5(x,y);//for Debug
	    return p.move(x, y, 0);
	  }


	  //calculate keystoning distortion

	  private void calculateFactors() {
	    Xv = intersect2D(at[0], at[1], at[2], at[3]);
	    if (Xv!=null) {
	      Xax = at[0].sub(Xv);
	      S = at[3].sub(Xv).angle(Xax);
	      Sdir = (cross2D(at[3].sub(Xv), Xax)>0?1:-1);
	    } 
	    else {
	      sdY = signedDist2D(at[2],at[0],at[1]);
	    }
	    Yv = intersect2D(at[0], at[2], at[1], at[3]);
	    if (Yv!=null) {
	      Yax = at[0].sub(Yv);
	      T = at[3].sub(Yv).angle(Yax);
	      Tdir = (cross2D(at[3].sub(Yv), Yax)>0?1:-1);
	    } 
	    else {
	      sdX = signedDist2D(at[1],at[0],at[2]);
	    }
	  }


	  private Loc intersect2D(Loc a0, Loc a1, Loc b0, Loc b1) {
	    Loc a = a1.sub(a0);
	    Loc b = b1.sub(b0);
	    float d2 = cross2D(a,b);
	    if (PApplet.abs(d2)<1.e-2) return null;
	    return a1.sub(a.scale(cross2D(b, b1.sub(a1))/d2));
	  }


	  private float cross2D(Loc frm, Loc to) {
	    return frm.x*to.y-frm.y*to.x;
	  }


	  private float signedDist2D(Loc pt, Loc frm, Loc to) {
	    Loc b=pt.sub(frm);
	    Loc a=to.sub(frm);
	    float anorm = a.norm();
	    if (anorm<1.e-3) return pt.dist(frm); 
	    return cross2D(a, b)/anorm;
	  }



	  // for Debug
	  void drawP5(float x, float y) {
		  parent.stroke(0,64,0);
		  parent.line(x*parent.width,0,x*parent.width,parent.height);
	    parent.stroke(64,0,0);
	    parent.line(0,y*parent.height,parent.width,y*parent.height);
	   parent.noStroke();
	  }

	  void drawIr(Loc pen, int width, int height) {

		  parent.stroke(32);

	    if (Xv!=null) {
	    	parent.line(Xv.x*width, Xv.y*height, at[3].x*width, at[3].y*height);
	    	parent.line(Xv.x*width, Xv.y*height, at[0].x*width, at[0].y*height);
	    }

	    if (Yv!=null) {
	    	parent.line(Yv.x*width, Yv.y*height, at[3].x*width, at[3].y*height);
	    	parent.line(Yv.x*width, Yv.y*height, at[0].x*width, at[0].y*height);
	    }

	    parent.stroke(128);

	    parent.line(at[0].x*width,at[0].y*height,at[1].x*width,at[1].y*height);
	    parent.line(at[0].x*width,at[0].y*height,at[2].x*width,at[2].y*height);
	    parent.line(at[3].x*width,at[3].y*height,at[1].x*width,at[1].y*height);
	    parent.line(at[3].x*width,at[3].y*height,at[2].x*width,at[2].y*height);

	    if (pen.x>-1) {

	    	parent.stroke(128);

	      if (Xv!=null) {
	    	  parent.line(Xv.x*width, Xv.y*height, pen.x*width, pen.y*height);
	      }

	      parent.stroke(0,128,0);
	      if (Yv!=null) {
	    	  parent.line(Yv.x*width, Yv.y*height, pen.x*width, pen.y*height);
	      }
	    }
	    parent.noStroke();	
	  }

}


